#include <stdlib.h>
#include <stdio.h>
//#include <mem.h>
#include <string.h>
#include <stdarg.h>

//#define HSS_AKA
#define UE_AKA

#ifdef HSS_AKA
#include "ie.h"
#include "hss_sim_struct.h"
#endif

#define AU_MAX_K_LEN    16
#define AU_MAX_RAND_LEN 16
#define AU_MAX_XRES_LEN 8
#define AU_MAX_RES_LEN  8
#define AU_MAX_CK_LEN   16
#define AU_MAX_IK_LEN   16
#define AU_MAX_AUTN_LEN 16
#define	AU_MAX_AUTS_LEN 14
#define AU_MAX_SQN_LEN  6
#define AU_MAX_MAC_LEN  8
#define AU_MAX_MACS_LEN 8
#define AU_MAX_AK_LEN   6
#define AU_MAX_AMF_LEN  2

#include "app.h"

#ifdef HSS_AKA
extern tPrivateIdentityTable gPriIdTab[HSS_MAX_PRIID_CNT];
extern void AddMemoLine(int color, U8 *pszFormat, ...);
#else
typedef unsigned char U8;
static void AddMemoLine(int color, U8 *pszFormat, ...)
{
    char  result[320];
    va_list arglist;

    va_start(arglist, pszFormat);
    vsprintf(result, (char*)pszFormat, arglist);
    va_end(arglist);

    color = color;
    //STT_TRACE(-1,TL_MESSAGE,STT_LStr(result));
    //printf("%s\n", result);
}
#endif

int ConcealFlg = 1;
int UseInnerAuC = 1;
U8 AMF[AU_MAX_AMF_LEN] = {00, 00};

/*-------------------------------------------------------------------
* Example algorithms f1, f1*, f2, f3, f4, f5, f5*
*-------------------------------------------------------------------
*
* A sample implementation of the example 3GPP authentication and
* key agreement functions f1, f1*, f2, f3, f4, f5 and f5*. This is
* a byte-oriented implementation of the functions, and of the block
* cipher kernel function Rijndael.
*
* This has been coded for clarity, not necessarily for efficiency.
*
* The functions f2, f3, f4 and f5 share the same inputs and have
* been coded together as a single function. f1, f1* and f5* are
* all coded separately.
*
*-----------------------------------------------------------------*/

/*--------- Operator Variant Algorithm Configuration Field --------*/
/*------- Insert your value of OP here -------*/
U8 OP[16] = {0x63, 0xbf, 0xa5, 0x0e, 0xe6, 0x52, 0x33, 0x65,
		     0xff, 0x14, 0xc1, 0xf4, 0x5f, 0x88, 0x73, 0x7d};
/*------- Insert your value of OP here -------*/
/*--------------------------- prototypes --------------------------*/

void f1(U8 k[16], U8 rand[16], U8 sqn[6], U8 amf[2], U8 mac_a[8]);
void f2345(U8 k[16], U8 rand[16], U8 res[8], U8 ck[16], U8 ik[16], U8 ak[6]);
void f1star(U8 k[16], U8 rand[16], U8 sqn[6], U8 amf[2], U8 mac_s[8]);
void f5star(U8 k[16], U8 rand[16], U8 ak[6]);
void ComputeOPc(U8 op_c[16]);
void RijndaelKeySchedule(U8 key[16]);
void RijndaelEncrypt(U8 input[16], U8 output[16]);

/*-------------------------------------------------------------------
 * Function to compute OPc from OP and K. Assumes key schedule has
 * already been performed.
 *-----------------------------------------------------------------*/
void ComputeOPc(U8 op_c[16])
{
	U8 i;

	RijndaelEncrypt( OP, op_c );
	for (i=0; i<16; i++)
	op_c[i] ^= OP[i];
	return;
} /* end of function ComputeOPc */


/*-------------------- Rijndael round subkeys ---------------------*/
U8 roundKeys[11][4][4];
/*--------------------- Rijndael S box table ----------------------*/

U8 S[256] =
{
	99,124,119,123,242,107,111,197, 48, 1,103, 43,254,215,171,118,
	202,130,201,125,250, 89, 71,240,173,212,162,175,156,164,114,192,
	183,253,147, 38, 54, 63,247,204, 52,165,229,241,113,216, 49, 21,
	4,199, 35,195, 24,150, 5,154, 7, 18,128,226,235, 39,178,117,
	9,131, 44, 26, 27,110, 90,160, 82, 59,214,179, 41,227, 47,132,
	83,209, 0,237, 32,252,177, 91,106,203,190, 57, 74, 76, 88,207,
	208,239,170,251, 67, 77, 51,133, 69,249, 2,127, 80, 60,159,168,
	81,163, 64,143,146,157, 56,245,188,182,218, 33, 16,255,243,210,
	205, 12, 19,236, 95,151, 68, 23,196,167,126, 61,100, 93, 25,115,
	96,129, 79,220, 34, 42,144,136, 70,238,184, 20,222, 94, 11,219,
	224, 50, 58, 10, 73, 6, 36, 92,194,211,172, 98,145,149,228,121,
	231,200, 55,109,141,213, 78,169,108, 86,244,234,101,122,174, 8,
	186,120, 37, 46, 28,166,180,198,232,221,116, 31, 75,189,139,138,
	112, 62,181,102, 72, 3,246, 14, 97, 53, 87,185,134,193, 29,158,
	225,248,152, 17,105,217,142,148,155, 30,135,233,206, 85, 40,223,
	140,161,137, 13,191,230, 66,104, 65,153, 45, 15,176, 84,187, 22,
};

/*------- This array does the multiplication by x in GF(2^8) ------*/
U8 Xtime[256] =
{
	0, 2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24, 26, 28, 30,
	32, 34, 36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62,
	64, 66, 68, 70, 72, 74, 76, 78, 80, 82, 84, 86, 88, 90, 92, 94,
	96, 98,100,102,104,106,108,110,112,114,116,118,120,122,124,126,
	128,130,132,134,136,138,140,142,144,146,148,150,152,154,156,158,
	160,162,164,166,168,170,172,174,176,178,180,182,184,186,188,190,
	192,194,196,198,200,202,204,206,208,210,212,214,216,218,220,222,
	224,226,228,230,232,234,236,238,240,242,244,246,248,250,252,254,
	27, 25, 31, 29, 19, 17, 23, 21, 11, 9, 15, 13, 3, 1, 7, 5,
	59, 57, 63, 61, 51, 49, 55, 53, 43, 41, 47, 45, 35, 33, 39, 37,
	91, 89, 95, 93, 83, 81, 87, 85, 75, 73, 79, 77, 67, 65, 71, 69,
	123,121,127,125,115,113,119,117,107,105,111,109, 99, 97,103,101,
	155,153,159,157,147,145,151,149,139,137,143,141,131,129,135,133,
	187,185,191,189,179,177,183,181,171,169,175,173,163,161,167,165,
	219,217,223,221,211,209,215,213,203,201,207,205,195,193,199,197,
	251,249,255,253,243,241,247,245,235,233,239,237,227,225,231,229
};

/*-------------------------------------------------------------------
 * Rijndael key schedule function. Takes 16-byte key and creates
 * all Rijndael's internal subkeys ready for encryption.
 *-----------------------------------------------------------------*/
void RijndaelKeySchedule( U8 key[16] )
{
	U8 roundConst;
	int i, j;

	/* first round key equals key */
	for (i=0; i<16; i++)
		roundKeys[0][i & 0x03][i>>2] = key[i];
	roundConst = 1;

	/* now calculate round keys */
	for (i=1; i<11; i++)
	{
		roundKeys[i][0][0] = S[roundKeys[i-1][1][3]]
					^ roundKeys[i-1][0][0] ^ roundConst;
		roundKeys[i][1][0] = S[roundKeys[i-1][2][3]]
					^ roundKeys[i-1][1][0];
		roundKeys[i][2][0] = S[roundKeys[i-1][3][3]]
					^ roundKeys[i-1][2][0];
		roundKeys[i][3][0] = S[roundKeys[i-1][0][3]]
					^ roundKeys[i-1][3][0];
		for (j=0; j<4; j++)
		{
			roundKeys[i][j][1] = roundKeys[i-1][j][1] ^ roundKeys[i][j][0];
			roundKeys[i][j][2] = roundKeys[i-1][j][2] ^ roundKeys[i][j][1];
			roundKeys[i][j][3] = roundKeys[i-1][j][3] ^ roundKeys[i][j][2];
		}

		/* update round constant */
		roundConst = Xtime[roundConst];
	}
	return;
} /* end of function RijndaelKeySchedule */

/* Round key addition function */
void KeyAdd(U8 state[4][4], U8 roundKeys[11][4][4], int round)
{
	int i, j;

	for (i=0; i<4; i++)
		for (j=0; j<4; j++)
			state[i][j] ^= roundKeys[round][i][j];
	return;
}

/* Byte substitution transformation */
int ByteSub(U8 state[4][4])
{
	int i, j;

	for (i=0; i<4; i++)
		for (j=0; j<4; j++)
			state[i][j] = S[state[i][j]];
	return 0;
}

/* Row shift transformation */
void ShiftRow(U8 state[4][4])
{
	U8 temp;

	/* left rotate row 1 by 1 */
	temp = state[1][0];
	state[1][0] = state[1][1];
	state[1][1] = state[1][2];
	state[1][2] = state[1][3];
	state[1][3] = temp;

	/* left rotate row 2 by 2 */
	temp = state[2][0];
	state[2][0] = state[2][2];
	state[2][2] = temp;
	temp = state[2][1];
	state[2][1] = state[2][3];
	state[2][3] = temp;

	/* left rotate row 3 by 3 */
	temp = state[3][0];
	state[3][0] = state[3][3];
	state[3][3] = state[3][2];
	state[3][2] = state[3][1];
	state[3][1] = temp;

	return;
}

/* MixColumn transformation*/
void MixColumn(U8 state[4][4])
{
	U8 temp, tmp, tmp0;
	int i;

	/* do one column at a time */
	for (i=0; i<4;i++)
	{
		temp = state[0][i] ^ state[1][i] ^ state[2][i] ^ state[3][i];
		tmp0 = state[0][i];

		/* Xtime array does multiply by x in GF2^8 */
		tmp = Xtime[state[0][i] ^ state[1][i]];
		state[0][i] ^= temp ^ tmp;
		tmp = Xtime[state[1][i] ^ state[2][i]];
		state[1][i] ^= temp ^ tmp;
		tmp = Xtime[state[2][i] ^ state[3][i]];
		state[2][i] ^= temp ^ tmp;
		tmp = Xtime[state[3][i] ^ tmp0];
		state[3][i] ^= temp ^ tmp;
	}
	return;
}

/*-------------------------------------------------------------------
 * Rijndael encryption function. Takes 16-byte input and creates
 * 16-byte output (using round keys already derived from 16-byte
 * key).
 *-----------------------------------------------------------------*/
void RijndaelEncrypt(U8 input[16], U8 output[16])
{
	U8 state[4][4];
	int i, r;

	/* initialise state array from input byte string */
	for (i=0; i<16; i++)
		state[i & 0x3][i>>2] = input[i];

	/* add first round_key */
	KeyAdd(state, roundKeys, 0);

	/* do lots of full rounds */
	for (r=1; r<=9; r++)
	{
		ByteSub(state);
		ShiftRow(state);
		MixColumn(state);
		KeyAdd(state, roundKeys, r);
	}

	/* final round */
	ByteSub(state);
	ShiftRow(state);
	KeyAdd(state, roundKeys, r);

	/* produce output byte string from state array */
	for (i=0; i<16; i++)
	{
		output[i] = state[i & 0x3][i>>2];
	}
	return;
} /* end of function RijndaelEncrypt */

/* Derive GSM sres from xres */
void c2(U8 xres[8], U8 sres[4])
{
    int i;

    for (i = 0; i < 4; i++)
    {
        sres[i] = xres[i] ^ xres[i+4];
    }
}

/* Derive GSM kc from ik/ck for interoperability with GSM */
void c3(U8 ck[16], U8 ik[16], U8 kc[8])
{
    int i;

    for (i = 0; i < 8; i++)
    {
        kc [i] = ck[i] ^ ck[i+8] ^ ik[i] ^ ik[i+8];
    }
}

/*-------------------------------------------------------------------
 * Algorithm f1
 *-------------------------------------------------------------------
 *
 * Computes network authentication code MAC-A from key K, random
 * challenge RAND, sequence number SQN and authentication management
 * field AMF.
 *
 *-----------------------------------------------------------------*/
void f1 ( U8 k[16], U8 rand[16], U8 sqn[6], U8 amf[2],
	  U8 mac_a[8] )
{
	U8 op_c[16];
	U8 temp[16];
	U8 in1[16];
	U8 out1[16];
	U8 rijndaelInput[16];
	U8 i;


	RijndaelKeySchedule( k );

	ComputeOPc( op_c );

	for (i=0; i<16; i++)
		rijndaelInput[i] = rand[i] ^ op_c[i];
	RijndaelEncrypt( rijndaelInput, temp );

	for (i=0; i<6; i++)
	{
		in1[i] = sqn[i];
		in1[i+8] = sqn[i];
	}

	for (i=0; i<2; i++)
	{
		in1[i+6] = amf[i];
		in1[i+14] = amf[i];
	}

	/* XOR op_c and in1, rotate by r1=64, and XOR *
	 * on the constant c1 (which is all zeroes) */

	for (i=0; i<16; i++)
		rijndaelInput[(i+8) % 16] = in1[i] ^ op_c[i];

	/* XOR on the value temp computed before */

	for (i=0; i<16; i++)
		rijndaelInput[i] ^= temp[i];

	RijndaelEncrypt( rijndaelInput, out1 );
	for (i=0; i<16; i++)
		out1[i] ^= op_c[i];
	for (i=0; i<8; i++)
		mac_a[i] = out1[i];
	return;
} /* end of function f1 */

/*-------------------------------------------------------------------
 * Algorithms f2-f5
 *-------------------------------------------------------------------
 *
 * Takes key K and random challenge RAND, and returns response RES,
 * confidentiality key CK, integrity key IK and anonymity key AK.
 *
 *-----------------------------------------------------------------*/
void f2345(U8 k[16], U8 rand[16], U8 res[8], U8 ck[16], U8 ik[16], U8 ak[6])
{
	U8 op_c[16];
	U8 temp[16];
	U8 out[16];
	U8 rijndaelInput[16];
	U8 i;

	RijndaelKeySchedule( k );
	ComputeOPc( op_c );

	for (i=0; i<16; i++)
		rijndaelInput[i] = rand[i] ^ op_c[i];
	RijndaelEncrypt( rijndaelInput, temp );

	/* To obtain output block OUT2: XOR OPc and TEMP, *
	 * rotate by r2=0, and XOR on the constant c2 (which *
	 * is all zeroes except that the last bit is 1). */
	for (i=0; i<16; i++)
		rijndaelInput[i] = temp[i] ^ op_c[i];
	rijndaelInput[15] ^= 1;

	RijndaelEncrypt( rijndaelInput, out );
	for (i=0; i<16; i++)
		out[i] ^= op_c[i];
	for (i=0; i<8; i++)
		res[i] = out[i+8];
	for (i=0; i<6; i++)
		ak[i] = out[i];

	/* To obtain output block OUT3: XOR OPc and TEMP,
	 * rotate by r3=32, and XOR on the constant c3 (which *
	 * is all zeroes except that the next to last bit is 1). */
	for (i=0; i<16; i++)
		rijndaelInput[(i+12) % 16] = temp[i] ^ op_c[i];
	rijndaelInput[15] ^= 2;

	RijndaelEncrypt( rijndaelInput, out );
	for (i=0; i<16; i++)
		out[i] ^= op_c[i];
	for (i=0; i<16; i++)
		ck[i] = out[i];

	/* To obtain output block OUT4: XOR OPc and TEMP, *
	 * rotate by r4=64, and XOR on the constant c4 (which *
	 * is all zeroes except that the 2nd from last bit is 1). */
	for (i=0; i<16; i++)
		rijndaelInput[(i+8) % 16] = temp[i] ^ op_c[i];
	rijndaelInput[15] ^= 4;

	RijndaelEncrypt( rijndaelInput, out );
	for (i=0; i<16; i++)
		out[i] ^= op_c[i];
	for (i=0; i<16; i++)
		ik[i] = out[i];
	return;
} /* end of function f2345 */

/*-------------------------------------------------------------------
 * Algorithm f1*
 *-------------------------------------------------------------------
 *
 * Computes resynch authentication code MAC-S from key K, random
 * challenge RAND, sequence number SQN and authentication management
 * field AMF.
 *
 *-----------------------------------------------------------------*/
void f1star(U8 k[16], U8 rand[16], U8 sqn[6], U8 amf[2], U8 mac_s[8])
{
	U8 op_c[16];
	U8 temp[16];
	U8 in1[16];
	U8 out1[16];
	U8 rijndaelInput[16];
	U8 i;

	RijndaelKeySchedule( k );
	ComputeOPc( op_c );

	for (i=0; i<16; i++)
		rijndaelInput[i] = rand[i] ^ op_c[i];
	RijndaelEncrypt( rijndaelInput, temp );

	for (i=0; i<6; i++)
	{
		in1[i] = sqn[i];
		in1[i+8] = sqn[i];
	}
	for (i=0; i<2; i++)
	{
		in1[i+6] = amf[i];
		in1[i+14] = amf[i];
	}

	/* XOR op_c and in1, rotate by r1=64, and XOR *
	 * on the constant c1 (which is all zeroes) */
	for (i=0; i<16; i++)
		rijndaelInput[(i+8) % 16] = in1[i] ^ op_c[i];

	/* XOR on the value temp computed before */
	for (i=0; i<16; i++)
		rijndaelInput[i] ^= temp[i];
	RijndaelEncrypt( rijndaelInput, out1 );

	for (i=0; i<16; i++)
		out1[i] ^= op_c[i];
	for (i=0; i<8; i++)
		mac_s[i] = out1[i+8];
	return;
} /* end of function f1star */

/*-------------------------------------------------------------------
 * Algorithm f5*
 *-------------------------------------------------------------------
 *
 * Takes key K and random challenge RAND, and returns resynch
 * anonymity key AK.
 *
 *-----------------------------------------------------------------*/
void f5star(U8 k[16], U8 rand[16], U8 ak[6])
{
	U8 op_c[16];
	U8 temp[16];
	U8 out[16];
	U8 rijndaelInput[16];
	U8 i;

	RijndaelKeySchedule( k );
	ComputeOPc( op_c );

	for (i=0; i<16; i++)
		rijndaelInput[i] = rand[i] ^ op_c[i];
	RijndaelEncrypt( rijndaelInput, temp );

	/* To obtain output block OUT5: XOR OPc and TEMP, *
	 * rotate by r5=96, and XOR on the constant c5 (which *
	 * is all zeroes except that the 3rd from last bit is 1). */
	for (i=0; i<16; i++)
		rijndaelInput[(i+4) % 16] = temp[i] ^ op_c[i];
	rijndaelInput[15] ^= 8;

	RijndaelEncrypt( rijndaelInput, out );
	for (i=0; i<16; i++)
		out[i] ^= op_c[i];
	for (i=0; i<6; i++)
		ak[i] = out[i];
	return;
} /* end of function f5star */

/* generate AUTN for Auth */
void fautn(U8 sqn[6], U8 ak[6], U8 amf[2], U8 mac_a[8], U8 autn[16])
{
    int i;

    for (i = 0; i < 6; i++)
    {
        autn[i] = sqn[i] ^ ak[i];
    }

    for (i= 6; i < 8; i++)
    {
        autn[i] = amf[i-6];
    }

    for (i = 8; i < 16; i++)
    {
        autn[i] = mac_a[i-8];
    }
}

#ifdef HSS_AKA
/* Get sqn and macs from auts used in AuC */
void fauts(U8 auts[14], U8 sqn[6], U8 ak[6], U8 mac_s[8])
{
    int i;

    for (i = 0; i < 6; i++)
    {
        sqn[i] = auts[i] ^ ak[i];
    }

    for (i = 6; i < 14; i++)
    {
        mac_s[i-6] = auts[i];
    }
}
#else
/* Gen AUTS used in UE */
void fauts(U8 auts[14], U8 sqn[6], U8 ak[6], U8 mac_s[8])
{
    int i;

    for (i = 0; i < 6; i++)
    {
        auts[i] = sqn[i] ^ ak[i];
    }

    for (i = 6; i < 14; i++)
    {
        auts[i] = mac_s[i-6];
    }
}
#endif


static int ShowDebugInfo = 1;
void PrintBinCode(U8 *Name, U8 *Bin, U8 len)
{
    int i;
    U8 tmp[160], tmp1[4];

    if(!ShowDebugInfo)
        return;

    strcpy((char*)tmp, (char*)Name);
    for(i=0; i<len; i++)
    {
        sprintf((char*)tmp1, "%02x ", Bin[i]);
        strcat((char*)tmp, (char*)tmp1);
    }
    AddMemoLine(0, (U8*)"%s", tmp);
}

void PrintStrCode(U8 *Name, U8 *Str)
{
    U8 tmp[160];

    if(!ShowDebugInfo)
        return;

    strcpy((char*)tmp, (char*)Name);
    strcat((char*)tmp, (char*)Str);
    AddMemoLine(0, (U8*)"%s", tmp);
}

#ifdef HSS_AKA
void PrintAuthData(U32 n)
{
    int i, j;
    U8 tmp[80], tmp1[4];

    AddMemoLine(0, "====AuthData dump for PrivateID: %d====", n);
    strcpy(tmp, "16 bits K used: ");
    for(i=0; i<AU_MAX_K_LEN; i++)
    {
        sprintf(tmp1, "%02x ", gPriIdTab[n].K[i]);
        strcat(tmp, tmp1);
    }
    AddMemoLine(0, "%s", tmp);

    strcpy(tmp, "Next 6 bits SQN: ");
    for(i=0; i<AU_MAX_SQN_LEN; i++)
    {
        sprintf(tmp1, "%02x ", gPriIdTab[n].SQN[i]);
        strcat(tmp, tmp1);
    }
    AddMemoLine(0, "%s", tmp);

    for(i=0; i<PRIID_MAX_AUTHDATA_CNT; i++)
    {
        if(gPriIdTab[n].pAuthData[i].IsUsed == 0)
            break;
        AddMemoLine(0, "Auth Vector %d:", i);
        AddMemoLine(0, "    AUTN: %s", gPriIdTab[n].pAuthData[i].AUTN);
        AddMemoLine(0, "    XRES: %s", gPriIdTab[n].pAuthData[i].XRES);
        AddMemoLine(0, "    RAND: %s", gPriIdTab[n].pAuthData[i].RAND);
        AddMemoLine(0, "    CK  : %s", gPriIdTab[n].pAuthData[i].CK);
        AddMemoLine(0, "    IK  : %s", gPriIdTab[n].pAuthData[i].IK);
    }

    AddMemoLine(0, "====End AuthData dump====");
}
#endif

void ConvertBinToStr(U8 *Bin, U8 len, U8 *Str)
{
    char ch0,ch1;
	U8 i, t;

	Str[len*2] = 0;
	for(i=0; i<len; i++)
    {
		t = (Bin[i] & 0xF0) >> 4;
		if(t < 0x0A)
            ch0 = t + '0';
		else
            ch0 = t - 0x0A + 'A';
		t = Bin[i] & 0x0F;
		if(t < 0x0A)
            ch1 = t + '0';
		else
            ch1 = t - 0x0A + 'A';
		Str[2*i] = ch0;
		Str[2*i + 1] = ch1;
	}
}

void ConvertStrToBin(U8 *Str, U8 *Bin, U8 len)
{
    char ch0, ch1;
	U8	i0, i1, i, nlen;

    memset(Bin, 0, len);
	nlen = strlen((char*)Str) / 2;
	if(nlen >= len)
        nlen = len;
	for(i=0; i<nlen; i++)
    {
		ch0 = Str[2*i];
		ch1 = Str[2*i + 1];
        i0 = i1 = 0;
		if(ch0 <= '9' && ch0 >= '0')
            i0 = ch0 - '0';
		else if(ch0 <= 'F' && ch0 >= 'A')
            i0 = ch0 - 'A' + 0x0A;
		else if(ch0 <= 'f' && ch0 >= 'a')
            i0 = ch0 - 'a' + 0x0A;

		if(ch1 <= '9' && ch1 >= '0')
            i1 = ch1 - '0';
		else if(ch1 <= 'F' && ch1 >= 'A')
            i1 = ch1 - 'A' + 0x0A;
		else if(ch1 <= 'f' && ch1 >= 'a')
            i1 = ch1 - 'a' + 0x0A;
		Bin[i] = (i0 & 0x0F) << 4 | (i1 & 0x0F);
	}
}

/* Generate 16 bytes random data */
void GenRand(U8 *val)
{
    U8  *p;
	int i, n;

	for(i = 0, p = val; i < 4; i ++)
	{
		n = rand();
		*p ++ = n & 0xff;
		*p ++ = (n >> 8) & 0xff;
		*p ++ = (n >> 16) & 0xff;
		*p ++ = (n >> 24) & 0xff;
	}
}

#ifdef HSS_AKA
/*-------------------------------------------------------------------
 * Generate Auth Vectors(Used in AuC)*
 *-------------------------------------------------------------------
 *
 * Generatr Num vectors with SQN and RAND, include reset SQN procedure
 * SQN definition: SQN = SEQ1||SEQ2||IND, Among them:
 *      SEQ1: 32bits;  SEQ2: 11bits; IND: 5bits
 * We simply set SEQ2=GLC=0 to implement none time-based profile
 *
 *-----------------------------------------------------------------*/
typedef union
{
    int i;
    unsigned char uc[4];
} uU;

void IncSEQ(U8 *SQN)
{
    uU u;

    /* inc seq */
    u.uc[0] = SQN[3];
    u.uc[1] = SQN[2];
    u.uc[2] = SQN[1];
    u.uc[3] = SQN[0];
    u.i = u.i + 1;
    SQN[0] = u.uc[3];
    SQN[1] = u.uc[2];
    SQN[2] = u.uc[1];
    SQN[3] = u.uc[0];

    /* if warp arround */
    if(u.i == 0)
        SQN[4] ++;
}

void IncIND(U8 *SQN)
{
    /* Increase IND in SQN */
    SQN[5] = (SQN[5] + 1) % 32;
}

void IncSQN(U8 *SQN)
{
    /* Increase IND in SQN */
    IncIND(SQN);

    /* Increase SEQ in SQN */
    IncSEQ(SQN);
}

void GenAuthVectors(U32 PriIdx, U8 *RAND, U8 *AUTS, U8 Num, U8 ResyncFlg)
{
	U8 SQN[AU_MAX_SQN_LEN];   // used for f5
	U8 AK[AU_MAX_AK_LEN];
    U8 MAC[AU_MAX_MAC_LEN];
    U8 nRAND[AU_MAX_RAND_LEN], nAUTN[AU_MAX_AUTN_LEN], nXRES[AU_MAX_XRES_LEN];
    U8 nAUTS[AU_MAX_AUTS_LEN], nIK[AU_MAX_IK_LEN], nCK[AU_MAX_CK_LEN];
    U8 *SQNhe, K[AU_MAX_K_LEN];
    tHssAuthData *pAuth;
    int i;

    if(PriIdx >= HSS_MAX_PRIID_CNT)
        return;

    SQNhe = gPriIdTab[PriIdx].SQN;
    PrintStrCode("Used K Str: ", gPriIdTab[PriIdx].K);
    ConvertStrToBin(gPriIdTab[PriIdx].K, K, AU_MAX_K_LEN);
    PrintBinCode("K Bin: ", K, AU_MAX_K_LEN);
    pAuth = gPriIdTab[PriIdx].pAuthData;

    /* Reset SQNhe while resync */
    if(ResyncFlg)
    {
    	if(!ConcealFlg)
	    {
		    // SQN is not concealed
		    memset(AK, 0, AU_MAX_AK_LEN);
	    }
	    else
	    {
            /* Convert to binary */
            ConvertStrToBin(RAND, nRAND, AU_MAX_RAND_LEN);
		    // SQN is concealed, need AK
		    f5star(K, nRAND, AK);
	    }
        ConvertStrToBin(AUTS, nAUTS, AU_MAX_AUTS_LEN);
	    fauts(nAUTS, SQN, AK, MAC);
        memcpy(SQNhe, SQN, AU_MAX_SQN_LEN);
        IncSQN(SQNhe);
    }

    for(i=0; i<PRIID_MAX_AUTHDATA_CNT; i++)
        pAuth[i].IsUsed = 0;

    /* Generate auth vectors */
    for(i=0; (i<Num)&&(i<PRIID_MAX_AUTHDATA_CNT); i++)
    {
        memset((char*)&(pAuth[i]), 0, sizeof(tHssAuthData));
        /* RAND */
        GenRand(nRAND);
        PrintBinCode("Generate RAND Bin: ", nRAND, AU_MAX_RAND_LEN);
        ConvertBinToStr(nRAND, AU_MAX_RAND_LEN, pAuth[i].RAND);
        PrintStrCode("RAND Str: ", pAuth[i].RAND);

        /* XRES, CK, IK */
        f1(K, nRAND, SQNhe, AMF, MAC);
        f2345(K, nRAND, nXRES, nCK, nIK, AK);

        PrintBinCode("Generate XRES Bin: ", nXRES, AU_MAX_XRES_LEN);
        ConvertBinToStr(nXRES, AU_MAX_XRES_LEN, pAuth[i].XRES);
        PrintStrCode("XRES Str: ", pAuth[i].XRES);

        PrintBinCode("Generate CK Bin: ", nCK, AU_MAX_CK_LEN);
        ConvertBinToStr(nCK, AU_MAX_CK_LEN, pAuth[i].CK);
        PrintStrCode("CK Str: ", pAuth[i].CK);

        PrintBinCode("Generate IK Bin: ", nIK, AU_MAX_IK_LEN);
        ConvertBinToStr(nIK, AU_MAX_IK_LEN, pAuth[i].IK);
        PrintStrCode("IK Str: ", pAuth[i].IK);

        if(!ConcealFlg)
            memset(AK, 0, AU_MAX_AK_LEN);
        else
            f5star(K, nRAND, AK);
        PrintBinCode("Generate AK Bin: ", AK, AU_MAX_AK_LEN);

        /* AUTN */
        PrintBinCode("Used SQN Bin: ", SQNhe, AU_MAX_SQN_LEN);
        PrintBinCode("Used MAC Bin: ", MAC, AU_MAX_MAC_LEN);
        fautn(SQNhe, AK, AMF, MAC, nAUTN);
        PrintBinCode("Generate AUTN Bin: ", nAUTN, AU_MAX_AUTN_LEN);
        ConvertBinToStr(nAUTN, AU_MAX_AUTN_LEN, pAuth[i].AUTN);
        PrintStrCode("AUTN Str: ", pAuth[i].AUTN);

        IncSEQ(SQNhe); /* Keep the same IND in a batch */
        pAuth[i].IsUsed = 1;
    }
    IncIND(SQNhe);
}
#endif

#ifdef UE_AKA
/*-------------------------------------------------------------------
 * UE authorization procedure(Used in UE)*
 *-------------------------------------------------------------------
 *
 * Input: K, SQNms, RAND, AUTN;
 * output: IK, CK, RES or NULL or AUTS;
 * return: 0(Succ) or 1(Reject) or 2(Resync)
 * 1. Retrieve SQN: AK=f5k(RAND), SQN=(SQN^AK)^AK
 * 2. Computes XMAC=f1k(SQN||RAND||AMF)
 * 3. Compares XMAC with MAC in AUTN, if not OK, return 1(Auth Reject)
 * 4. Verifies SQN is in correct range and freshed,
 *    otherwise computes AUTS: AUTS=Conc(SQNms)||MACS and return 2(ReSync)
 * 5. Computes IK, CK, RES, and return 0.
 *
 *-----------------------------------------------------------------*/
// 请将以下两项加入到每一个用户对应的数据结构中
//U8 K[AU_MAX_K_LEN*2+1]; /* Authorization key display string */
//U8 SQNms[32][AU_MAX_SQN_LEN]; /* Restore lastest 32 SQNs */

int CompareBinCode(U8 *s1, U8 *s2, U8 len)
{
    return memcmp((char*)s1, (char*)s2, len);
}

typedef struct{
	U8 v[32][AU_MAX_SQN_LEN];
}SQNms_Stru;
int UENetworkAuthorize(U8 *K, U8 *SQNms, U8 *RAND, U8 *AUTN, U8 *IK, U8 *CK, U8 *RES, U8 *AUTS)
{
    U8 SQN[AU_MAX_SQN_LEN], ConcSQN[AU_MAX_SQN_LEN];
    U8 AK[AU_MAX_AK_LEN];
    U8 MAC[AU_MAX_MAC_LEN], nXMAC[AU_MAX_MAC_LEN], nMACS[AU_MAX_MACS_LEN];
    U8 nRAND[AU_MAX_RAND_LEN], nAUTN[AU_MAX_AUTN_LEN];
    U8 nK[AU_MAX_K_LEN];
    U8 nIK[AU_MAX_IK_LEN], nCK[AU_MAX_CK_LEN];
    U8 nRES[AU_MAX_RES_LEN], nAUTS[AU_MAX_AUTS_LEN];
    U8 i, ind, ResyncFlg;

    SQNms_Stru *vv=(SQNms_Stru *)SQNms;	

    PrintStrCode((U8*)"Used K Str: ", K);
    ConvertStrToBin(K, nK, AU_MAX_K_LEN);
    PrintBinCode((U8*)"K Bin: ", nK, AU_MAX_K_LEN);

    PrintStrCode((U8*)"RAND: ", RAND);
    PrintStrCode((U8*)"AUTN: ", AUTN);
    ConvertStrToBin(RAND, nRAND, AU_MAX_RAND_LEN);
    ConvertStrToBin(AUTN, nAUTN, AU_MAX_AUTN_LEN);
    PrintBinCode((U8*)"Convert RAND Bin: ", nRAND, AU_MAX_RAND_LEN);
    PrintBinCode((U8*)"Convert AUTN Bin: ", nAUTN, AU_MAX_AUTN_LEN);

    if(!ConcealFlg)
        memset(AK, 0, AU_MAX_AK_LEN);
    else
        f5star(nK, nRAND, AK);
    PrintBinCode((U8*)"Generate AK Bin: ", AK, AU_MAX_AK_LEN);

    /* Get Concealed SQN*/
    for(i=0; i<AU_MAX_SQN_LEN; i++)
        SQN[i] = nAUTN[i] ^ AK[i];
    PrintBinCode((U8*)"SQN: ", SQN, AU_MAX_SQN_LEN);

    /* Compute XMAC */
    f1(nK, nRAND, SQN, AMF, nXMAC);
    for(i=0; i<AU_MAX_MAC_LEN; i++)
        MAC[i] = nAUTN[8+i];
    if(0 != CompareBinCode(nXMAC, MAC, AU_MAX_MAC_LEN))
    {
        AddMemoLine(0, (U8*)"XMAC not match, authorization rejected!");
        return 1;
    }

    /* Verifies SQN */
    ResyncFlg = 0;
    ind = SQN[5] & 0x1f;
    PrintBinCode((U8*)"SQNms: ",(U8*)(*vv).v[ind], AU_MAX_SQN_LEN);

    if(0 > CompareBinCode(SQN, (*vv).v[ind], AU_MAX_SQN_LEN))
    {
        /* SQN not in correct range, need resync */
        ResyncFlg = 1;
        for(i=0; i<AU_MAX_SQN_LEN; i++)
            ConcSQN[i] = (*vv).v[ind][i]^ AK[i];

        f1star(nK, nRAND, (*vv).v[ind], AMF, nMACS);

        for(i=0; i<6; i++)
            nAUTS[i] = ConcSQN[i];
        for(i=0; i<8; i++)
            nAUTS[6+i] = nMACS[i];
        ConvertBinToStr(nAUTS, AU_MAX_AUTS_LEN, AUTS);
        PrintStrCode((U8*)"Generate AUTS: ", AUTS);
        AddMemoLine(0, (U8*)"SQN verified failed, return AUTS and RAND!");
        return 2;
    }

    /* Restore recieved SQN */
    for(i=0; i<AU_MAX_SQN_LEN; i++)
        (*vv).v[ind][i] = SQN[i];

    PrintBinCode((U8*)"SQNms: ",(U8*)(*vv).v[ind], AU_MAX_SQN_LEN);

    /* Generate RES, IK, CK */
    f2345(nK, nRAND, nRES, nCK, nIK, AK);
    ConvertBinToStr(nRES, AU_MAX_RES_LEN, RES);
    ConvertBinToStr(nCK, AU_MAX_CK_LEN, CK);
    ConvertBinToStr(nIK, AU_MAX_IK_LEN, IK);
    PrintStrCode((U8*)"Generate RES: ", RES);
    PrintStrCode((U8*)"Generate CK: ", CK);
    PrintStrCode((U8*)"Generate IK: ", IK);
    AddMemoLine(0, (U8*)"Authorization successfully, return RES.");
    return 0;
}
#endif

void testgenauthdata()
{
    int n;
    U8 AUTS[29], RES[17], CK[33], IK[33];
    U8 SQNms[32][AU_MAX_SQN_LEN];
    U8 K[33];
    U8 RAND[33], AUTN[33];

    n = 4;
#ifdef HSS_AKA
    GenAuthVectors(0, NULL, NULL, n, 0);
    PrintAuthData(0);
#ifdef UE_AKA
    memset(SQNms, 0, 32*AU_MAX_SQN_LEN);
    for(i=0; i<n; i++)
        UENetworkAuthorize(gPriIdTab[0].K, SQNms, gPriIdTab[0].pAuthData[i].RAND, gPriIdTab[0].pAuthData[i].AUTN, IK, CK, RES, AUTS);
#endif
#else
    memset(SQNms, 0, 32*AU_MAX_SQN_LEN);
    strcpy((char*)K, "0123456789abcdef0123456789abcdef");
    strcpy((char*)RAND, "0123456789abcdef0123456789abcdef");
    strcpy((char*)AUTN, "0123456789abcdef0123456789abcdef");
    UENetworkAuthorize(K, (U8*)SQNms, RAND, AUTN, IK, CK, RES, AUTS);
#endif
}

